package tracker;
/****************************************************************************************************************************
* TrackerThread implements javas concept of a virtual input device which is added to the 
* scene graph.  This allows Java3d to call the tracker data methods.																							*
*																															*
* Written by Ross Smith - Jan 2005																							*
*																															*
****************************************************************************************************************************/

import javax.media.j3d.InputDevice;
import javax.media.j3d.Sensor;
import javax.media.j3d.SensorRead;
import javax.media.j3d.Transform3D;
import javax.vecmath.Quat4d;
import javax.vecmath.Vector3f;


public class TrackerThread implements InputDevice{

	private final double rad = Math.PI / 180.0;
	
	/*Enable/Disable debugging output*/
	private boolean DEBUG = false;
	
    /*Store all the tracking data internally*/
    private double heading[];
    private double pitch[];
    private double roll[];
    private double X[];
    private double Y[];
    private double Z[];
    
    private int sensorCount;
    private TrackerSerialPort sport;
    private TrackerPolhemus tracker;
    private Sensor sensors[];
    private SensorRead sensorRead[];
    private Transform3D []newTransform;
    
    private Transform3D rotTrans = new Transform3D();
    private Vector3f position = new Vector3f();
    private Vector3f basePos = new Vector3f();
    
    /*Coordinates of the tracker base*/
    private float tbase_x = 0.0f; 
    private float tbase_y = 0.0f;
    private float tbase_z = 0.0f;
    
    private int fpsCount = 0;
    private int fps = 0;
    private long lastUpdate;
    
    public TrackerThread (TrackerPolhemus tracker, TrackerSerialPort sport) {
    	
    	this.tracker = tracker;
    	this.sport = sport;
    	sensorCount = tracker.getNumSensors();
		
    	heading = new double[sensorCount];
		pitch = new double[sensorCount];
		roll = new double[sensorCount];
		X = new double[sensorCount];
		Y = new double[sensorCount];
		Z = new double[sensorCount];
		
		this.sensors = new Sensor[this.sensorCount];
		this.sensorRead = new SensorRead[this.sensorCount];
		this.newTransform = new Transform3D[this.sensorCount];
		for(int i=0;i<this.sensorCount;i++){
			this.sensors[i] = new Sensor(this);
			this.sensorRead[i] = new SensorRead();
			this.newTransform[i] = new Transform3D();
		}
		
		//FixMe
		this.sensors[0].setPredictionPolicy(Sensor.HEAD_PREDICTOR);
		this.sensors[1].setPredictionPolicy(Sensor.HAND_PREDICTOR);
		this.sensors[0].setPredictor(Sensor.PREDICT_NEXT_FRAME_TIME);
		
		basePos.set(tbase_x, tbase_y, tbase_z);
		this.lastUpdate = System.currentTimeMillis();
    }
     
    public Sensor getNonDominantHandTrackerSensor () {    
    	if(this.sensorCount>=3)
    		return sensors[2];
    	else
    		return null;
    }

    private long deadTime;						/*Record of the time the FPS dropped to zero*/
    private boolean timingDeadTime = false;		/*Intermediate state used when we are deciding if the tracker is dead*/
    private long noUpdateTimeout = 800;			/*Length of time we use to decide if the tracker is dead in miliseconds*/
    private boolean reAquiringTracker = false;	/*Indicates if we are trying to reaquire the tracker*/
    private int reaquireRetryCount = 0;			/*Count for the number of times we have tried to reaquire the tracker*/
    
    public void pollAndProcessInput(){
    	this.processStreamInput();
    	
		if(fps == 0){
			if(timingDeadTime != true){
				deadTime = System.currentTimeMillis();
				timingDeadTime = true;
				this.reAquiringTracker = true;
			}
			else{
				if(System.currentTimeMillis() > deadTime + noUpdateTimeout){
					
					/*Do this first so the screen updates*/
					reaquireRetryCount++;				
					
					/*Get trackers initilisation string*/
					String init = this.tracker.getInitilisationString();
					
					/*Clean out any old records*/
					this.sport.flush();
					
					/*Request the firmware number*/
					sport.write("S");
					
					/*Check for a response for .5s*/
					String response = "";
					long startTime = System.currentTimeMillis();
					long timeOut = 500;
					
					while(System.currentTimeMillis() < (startTime + timeOut)){
						response = response.concat(sport.readTimed(1, 50));	
					}
					
					/*See if the tracker is in a ready state and responding correctly*/
					if(response.substring(2,6).compareTo(tracker.getFirmwareIdent()) == 0){
						/*Configure the tracker*/
						for(int length=0;length<init.length();length++){
							sport.write(new Character(init.charAt(length)).toString());
				    		try{Thread.sleep(1);}catch(Exception e){}
				    	}
					}		
					/*reset so we dont try again for the timeout period*/
					this.deadTime = System.currentTimeMillis();
				}
			}
		}
		else{
			this.reAquiringTracker = false;
			reaquireRetryCount = 0;
			timingDeadTime = false;
		}    		
    }

    public int getRetryCount(){
    	return this.reaquireRetryCount;
    }
    
    public boolean getReaquiringTracker(){
    	return this.reAquiringTracker;
    }
    
    public int getFPS(){
    	return this.fps;
    }
    
    public void processStreamInput(){
		
    	updateSensors(); 
		
		for(int i = 0; i < sensorCount; i++) {
			sensorRead[i].setTime (System.currentTimeMillis());
			
			/* Tracker rotation code converted by Mark Rebane to use Quaternions for 
			 * the removal of Gimbal-Lock effect Calculate the quaternion rotation 
			 * from the euler values */
			double a = pitch[i] * rad,
				   b = heading[i] * rad,
				   c = -roll[i] * rad;
			
			Quat4d Qx = new Quat4d(Math.sin(a/2), 0, 0, Math.cos(a/2));		// Pitch
			Quat4d Qy = new Quat4d(0, Math.sin(b/2), 0, Math.cos(b/2));		// Heading
			Quat4d Qz = new Quat4d(0, 0, Math.sin(c/2), Math.cos(c/2));		// Roll
			Quat4d Q  = new Quat4d();
			/* Multiply the x, y, z Quaternions to get a final rotation Quaternion
			 * The order of multiplication is important Qy MUST be first!
			 * If Qx or Qz is first rotation for pitch or roll will be in the same World
			 * coordinate direction regardless of the users heading. */
			Q.mul(Qy, Qx);
			Q.mul(Qz);
			
			rotTrans.set(Q);

			float scale = .0254f;
			position.set(tbase_x +(float)(Y [i] * scale),(float)(tbase_y + Z [i] * scale),
					tbase_z + (float)(X [i] * scale));
			
			newTransform[i].set (position);
			newTransform[i].mul (rotTrans);
			sensorRead[i].set (newTransform[i]);
			
			sensors[i].setNextSensorRead (sensorRead[i]);
		}
		
		if(DEBUG)
			for(int i=0;i< this.sensorCount;i++)
				System.out.println("X=" +X[i] + " Y=" + Y[i] + " Z=" + Z[i]);
    }

    
    public void setNominalPositionAndOrientation() {
           for (int i = 0; i < this.sensorCount; i++) {
              sensorRead[i].setTime (System.currentTimeMillis ());
              position.set (basePos);
              newTransform[i].set (position);
              sensorRead[i].set (newTransform[i]);
              sensors[i].setNextSensorRead (sensorRead[i]);
           }
        }
    
    public void setProcessingMode(int mode){
    }
    
    public void start(){
    	String init = this.tracker.getInitilisationString();
  
    	/*We need to sleep just for a bit at each CR so we might as well do it for every character*/
    	for(int length=0;length<init.length();length++){
    		sport.write(new Character(init.charAt(length)).toString());
    		try{Thread.sleep(1);}catch(Exception e){}
    	}
    }
    
    public void updateSensors(){
    	try{
    		/* Read the data string which should have two records one for each sensor */
    		String data = sport.read(this.tracker.getMessageLength());
    		
    		/*Update the polhemus FPS*/
        	if(this.lastUpdate + 500 > System.currentTimeMillis()){
        		/*Dont add to the FPS if there is no data but keep calculating it*/
        		if(data!=null)
        			this.fpsCount += 2;
        	} else {
        		this.fps = fpsCount;
        		this.fpsCount = 0;
        		this.lastUpdate = System.currentTimeMillis();
        	}
        	
    		if(data == null)
    			return;
    			
    		//String data = "0000000000000000000000000000000000000000000000000000000000000000000000\n0000000000000000000000000000000000000000000000000000000000000000000000000000000000000000000";
    		/*Store each sensors data seperatly*/
    		String sensorData[] = new String[this.tracker.getNumSensors()];
    		
    		/*Find the index of the CR so we know where to split the string*/
    		int sensorDataIndex = 0;
    		int offset = 0;
    		for (int i=0; i < data.length(); i++){
    			if ((byte)data.charAt(i)==13){ /*When we find a CR*/
    				sensorData[sensorDataIndex++] = data.substring(offset, i).trim();
    				offset = i+1; /*Cut off the CR from the string*/
    			}
    		}
    		
    		if(DEBUG)
    			for (int i = 0; i < this.tracker.getNumSensors(); i++)
    				System.out.println("Sensor string = " + i + " = "+ sensorData[i]);
    		
    		/* Now parse all the sensors data */
    		for(int sensor=0;sensor < this.tracker.getNumSensors();sensor++){
    			
    			/* Use the sensor number supplied by the tracker this is a random starting index */
	    		int sensorNumber = Integer.parseInt(sensorData[sensor].substring(0, 2));	
	    		
	    		/*Base offset which varies depenging on the tracker*/
	    		int o = this.tracker.getPositionOffset();
	    		
	    		double heading = Double.parseDouble(sensorData[sensor].substring(o, o + 7));
	    		double pitch = Double.parseDouble(sensorData[sensor].substring(o + 7, o + 14));
	    		double roll = Double.parseDouble(sensorData[sensor].substring(o + 14, o + 21));
	    		
	    		double x = Double.parseDouble(sensorData[sensor].substring(o+21, o+28));
	    		double y = Double.parseDouble(sensorData[sensor].substring(o+28, o+35));
	    		double z = Double.parseDouble(sensorData[sensor].substring(o+35, o+42));
	    		
	    		this.heading[sensorNumber - 1] = heading;
	    		this.pitch[sensorNumber - 1] = pitch;
	    		
	    		/* when we are setting the head allow for the orientation of the sensor */
	    		if ((sensorNumber - 1) == 0){
	    			this.roll[sensorNumber - 1] = - roll + 180;
	    		} else {
	    			/* Not sure why this needs to be done, I think it might be because 
	    			 * of the hemisphere setup for each of the trackers is different */
	    			//if (this.tracker.getTrackerType() == "IsotrakII")
	    			this.roll[sensorNumber - 1] = -roll;
	    			//else if (this.tracker.getTrackerType() == "Fastrak")
	    			//this.roll[sensorNumber - 1] = -roll;
	    		}
	    			
	    		double scale = 1;
	    		
	    		if(this.tracker.getTrackerType() == "IsotrakII"){
	    			scale = 2;
	    			this.X[sensorNumber - 1] = x * scale;
	    			this.Y[sensorNumber - 1] = y * scale;
	    			this.Z[sensorNumber - 1] = z * scale;
	    		}
	    		else if(this.tracker.getTrackerType() == "Fastrak"){
	    			scale = 1;
	    			this.X[sensorNumber - 1] = x * scale;
	    			this.Y[sensorNumber - 1] = y * scale;
	    			this.Z[sensorNumber - 1] = z * scale;
	    		}
    		}
    		
    		if(DEBUG)
	    		for(int allSensors=0; allSensors< this.tracker.getNumSensors();allSensors++){
	    			
		    		System.out.print((allSensors+1) + " - [Heading = " + this.heading[allSensors]);
		    		System.out.print("] [Pitch = " + this.pitch[allSensors]);
		    		System.out.print("] [Roll = " + this.roll[allSensors] + "]");
		    		
		    		System.out.print("[X = " + this.X[allSensors]);
		    		System.out.print("] [Y = " + this.Y[allSensors]);
		    		System.out.println("] [Z = " + this.Z[allSensors] + "]\n");
	    		}

    	}
    	catch(Exception e){
    		/*Only print output debuggubg data while debugging, it will make FPS slow*/
    		if(DEBUG)
    			e.printStackTrace();
    	}
    
    }
    
    public Sensor getSensor(int channel){
    	return sensors[channel];
    }
    
    public void close(){
    	
    }
    
    public int getProcessingMode(){
    	return InputDevice.NON_BLOCKING;
    }
    
    public int getSensorCount(){
    	return this.sensorCount;
    }
    
    public boolean initialize() {   	
    	return true;
    }
    
    public Sensor getDominantHandTrackerSensor () {   
    	return sensors[1];
    }
    
    public TrackerPolhemus getTracker(){
    	return this.tracker;
    }
}
